﻿Imports Microsoft.Xna.Framework

''' <summary> 
''' Basic 3D Object 
''' </summary> 
Public MustInherit Class XNAObject
    Implements ITrackableObject

#Region "Variables"

    ''' <summary> 
    ''' The Object's Position in 3D Space 
    ''' </summary> 
    Private mPosition As Vector3

    ''' <summary> 
    ''' The Object's Rotation in 3D Space 
    ''' </summary> 
    Private mRotation As Quaternion = Quaternion.Identity

    ''' <summary> 
    ''' The Objects Scale 
    ''' </summary> 
    Private mScale As Vector3

    ''' <summary>
    ''' The manager that this object belongs to.
    ''' </summary>
    Private mGameManager As GameManager

    Private mMatricesValid As Boolean = False

#End Region

#Region "Public Properties"

    ''' <summary> 
    ''' The Object's Position in 3D Space 
    ''' </summary> 
    Public Property Position() As Vector3 Implements ITrackableObject.Position
        Get
            Return mPosition
        End Get
        Set(ByVal value As Vector3)
            If Not mPosition = value Then
                mPosition = value
                InvalidateMatrices()
            End If
        End Set
    End Property

    Public Property PositionX() As Single
        Get
            Return mPosition.X
        End Get
        Set(ByVal value As Single)
            If Not mPosition.X = value Then
                mPosition.X = value
                InvalidateMatrices()
            End If
        End Set
    End Property

    Public Property PositionY() As Single
        Get
            Return mPosition.Y
        End Get
        Set(ByVal value As Single)
            If Not mPosition.Y = value Then
                mPosition.Y = value
                InvalidateMatrices()
            End If
        End Set
    End Property

    Public Property PositionZ() As Single
        Get
            Return mPosition.Z
        End Get
        Set(ByVal value As Single)
            If Not mPosition.Z = value Then
                mPosition.Z = value
                InvalidateMatrices()
            End If
        End Set
    End Property

    ''' <summary> 
    ''' The Object's Rotation in 3D Space 
    ''' </summary> 
    Public Property Rotation() As Quaternion Implements ITrackableObject.Rotation
        Get
            Return mRotation
        End Get
        Set(ByVal value As Quaternion)
            If Not mRotation = value Then
                mRotation = value
                InvalidateMatrices()
            End If
        End Set
    End Property

    ''' <summary> 
    ''' The Object's Scale 
    ''' </summary> 
    Public Property Scale() As Vector3
        Get
            Return mScale
        End Get
        Set(ByVal value As Vector3)
            If Not mScale = value Then
                mScale = value
                InvalidateMatrices()
            End If
        End Set
    End Property

    ''' <summary>
    ''' The manager that this object belongs to.
    ''' </summary>
    Public Property GameManager() As GameManager
        Get
            GameManager = mGameManager
        End Get
        Set(ByVal value As GameManager)
            mGameManager = value
        End Set
    End Property

    Protected Property MatricesValid() As Boolean
        Get
            MatricesValid = mMatricesValid
        End Get
        Set(ByVal value As Boolean)
            mMatricesValid = value
        End Set
    End Property

#Region " Direction Properties "
    ' Direction Properties code translated from C# code supplied by Mike Lopez   

    ''' <summary> 
    ''' The forward direction of the rotated coordinate space expressed as 
    ''' a unit vector in world coordinate space 
    ''' </summary> 
    Public ReadOnly Property Forward() As Vector3 Implements ITrackableObject.Forward
        Get
            Forward.X = -2.0F * ((mRotation.X * mRotation.Z) + (mRotation.W * mRotation.Y))
            Forward.Y = 2.0F * ((mRotation.W * mRotation.X) - (mRotation.Y * mRotation.Z))
            Forward.Z = ((mRotation.X * mRotation.X) + (mRotation.Y * mRotation.Y)) - ((mRotation.Z * mRotation.Z) + (mRotation.W * mRotation.W))

            Forward = Vector3.Normalize(Forward)
        End Get
    End Property

    ''' <summary> 
    ''' The Up direction of the rotated coordinate space expressed as 
    ''' a unit vector in world coordinate space 
    ''' </summary> 
    Public ReadOnly Property Up() As Vector3 Implements ITrackableObject.Up
        Get
            Up.X = 2.0F * ((mRotation.X * mRotation.Y) - (mRotation.Z * mRotation.W))
            Up.Y = ((mRotation.Y * mRotation.Y) + (mRotation.W * mRotation.W)) - ((mRotation.Z * mRotation.Z) + (mRotation.X * mRotation.X))
            Up.Z = 2.0F * ((mRotation.Y * mRotation.Z) + (mRotation.X * mRotation.W))

            Up = Vector3.Normalize(Up)
        End Get
    End Property

    ''' <summary> 
    ''' The Right direction of the rotated coordinate space expressed as 
    ''' a unit vector in world coordinate space 
    ''' </summary> 
    Public ReadOnly Property Right() As Vector3
        Get
            Right.X = ((mRotation.X * mRotation.X) + (mRotation.W * mRotation.W)) - ((mRotation.Z * mRotation.Z) + (mRotation.Y * mRotation.Y))
            Right.Y = 2.0F * ((mRotation.X * mRotation.Y) + (mRotation.Z * mRotation.W))
            Right.Z = 2.0F * ((mRotation.X * mRotation.Z) - (mRotation.Y * mRotation.W))

            Right = Vector3.Normalize(Right)
        End Get
    End Property
#End Region ' Direction Properties

#End Region ' Public Properties

#Region "Constructor"

    ''' <summary> 
    ''' Create Object Instance 
    ''' </summary> 
    Public Sub New(ByVal GameManager As GameManager)
        mGameManager = GameManager
        mPosition = New Vector3(0, 0, 0)
        mRotation = New Quaternion(0, 0, 0, 1)
        mScale.X = 1.0F
        mScale.Y = 1.0F
        mScale.Z = 1.0F
    End Sub

#End Region

#Region "Public Methods"

    ''' <summary>
    ''' Move Object on all 3 axies
    ''' </summary>
    ''' <param name="distance">Distance to Translate Object</param>
    Public Overridable Sub Move(ByVal distance As Vector3)
        Translate(distance)
    End Sub

    ''' <summary> 
    ''' Move Object Forwards (Object's Z Axis) 
    ''' </summary> 
    ''' <param name="distance">Distance to Translate Object</param> 
    Public Overridable Sub MoveForwards(ByVal distance As Single)
        Translate(New Vector3(0.0F, 0.0F, -distance))
    End Sub

    ''' <summary> 
    ''' Move Object Backwards (Object's Z Axis) 
    ''' </summary> 
    ''' <param name="distance">Distance to Translate Object</param> 
    Public Overridable Sub MoveBackwards(ByVal distance As Single)
        Translate(New Vector3(0.0F, 0.0F, distance))
    End Sub

    ''' <summary> 
    ''' Strafe Object Left (Object's X Axis) 
    ''' </summary> 
    ''' <param name="distance">Distance to Translate Object</param> 
    Public Overridable Sub StrafeLeft(ByVal distance As Single)
        Translate(New Vector3(-distance, 0.0F, 0.0F))
    End Sub

    ''' <summary> 
    ''' Strafe Object Right (Object's X Axis) 
    ''' </summary> 
    ''' <param name="distance">Distance to Translate Object</param> 
    Public Overridable Sub StrafeRight(ByVal distance As Single)
        Translate(New Vector3(distance, 0.0F, 0.0F))
    End Sub

    ''' <summary> 
    ''' Strafe Object Up (Object's Y Axis) 
    ''' </summary> 
    ''' <param name="distance">Distance to Translate Object</param> 
    Public Overridable Sub StrafeUp(ByVal distance As Single)
        Translate(New Vector3(0.0F, distance, 0.0F))
    End Sub

    ''' <summary> 
    ''' Strafe Object Down (Object's Y Axis) 
    ''' </summary> 
    ''' <param name="distance">Distance to Translate Object</param> 
    Public Overridable Sub StrafeDown(ByVal distance As Single)
        Translate(New Vector3(0.0F, -distance, 0.0F))
    End Sub

    ''' <summary> 
    ''' Roll Object Left (Object's Z Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub RollLeft(ByVal angleDegrees As Single)
        Rotate(Vector3.Forward, MathHelper.ToRadians(-angleDegrees))
    End Sub

    ''' <summary> 
    ''' Roll Object Right (Object's Z Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub RollRight(ByVal angleDegrees As Single)
        Rotate(Vector3.Forward, MathHelper.ToRadians(angleDegrees))
    End Sub

    ''' <summary> 
    ''' Pitch Object Down (Object's X Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub PitchDown(ByVal angleDegrees As Single)
        Rotate(Vector3.Left, MathHelper.ToRadians(angleDegrees))
    End Sub

    ''' <summary> 
    ''' Pitch Object Up (Object's X Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub PitchUp(ByVal angleDegrees As Single)
        Rotate(Vector3.Left, MathHelper.ToRadians(-angleDegrees))
    End Sub

    ''' <summary> 
    ''' Rotate Object Left (Object's Y Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub RotateLeft(ByVal angleDegrees As Single)
        Rotate(Vector3.Up, MathHelper.ToRadians(angleDegrees))
    End Sub

    ''' <summary> 
    ''' Turn Object Right (Object's Y Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub RotateRight(ByVal angleDegrees As Single)
        Rotate(Vector3.Up, MathHelper.ToRadians(-angleDegrees))
    End Sub
    ''' <summary> 
    ''' Rotate Object Left (World's Y Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub RotateLeftAroundUp(ByVal angleDegrees As Single)
        Rotation = Quaternion.Normalize(Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(angleDegrees)) * Rotation)
    End Sub

    ''' <summary> 
    ''' Turn Object Right (World's Y Axis) 
    ''' </summary> 
    ''' <param name="angleDegrees">Angle to Rotate Object</param> 
    Public Overridable Sub RotateRightAroundUp(ByVal angleDegrees As Single)
        Rotation = Quaternion.Normalize(Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(-angleDegrees)) * Rotation)
    End Sub

    ''' <summary> 
    ''' Move Object towards Target Vector 
    ''' </summary> 
    ''' <param name="target">Target Vector</param> 
    ''' <param name="factor">Percentage of Translation</param> 
    Public Overridable Sub MoveTo(ByVal target As Vector3, ByVal factor As Single)
        Position = Vector3.Lerp(Position, target, factor)
    End Sub

    ''' <summary> 
    ''' Rotate Object towards Target Quaternion 
    ''' </summary> 
    ''' <param name="slerpTarget">Target Quaternion</param> 
    ''' <param name="factor">Percentage of Rotation</param> 
    Public Overridable Sub Slerp(ByVal slerpTarget As Quaternion, ByVal factor As Single)
        Rotation = Quaternion.Slerp(Rotation, slerpTarget, factor)
    End Sub

    ''' <summary> 
    ''' Rotate Object towards Target Vector 
    ''' </summary> 
    ''' <param name="target">Target Vector</param> 
    ''' <param name="factor">Percentage of Rotation</param> 
    Public Overridable Sub LookAtPoint(ByVal target As Vector3, ByVal factor As Single)
        Slerp(DirectionToPoint(Position, target), factor)
    End Sub

    ''' <summary> 
    ''' Force Object to Orient to UP Vector (Y) 
    ''' </summary> 
    ''' <param name="factor">Percentage of Rotation</param> 
    Public Sub ConstrainYAxis(ByVal factor As Single)
        Dim Q As New Quaternion(0, Rotation.Y, 0, Rotation.W)
        Slerp(Q, factor)
    End Sub

#End Region

#Region "Private Methods"

    ''' <summary> 
    ''' Rotate Object around specific Axis 
    ''' </summary>
    ''' <param name="axis">Axis to rotate around</param> 
    ''' <param name="angle">The angle to rotate around the specified axis</param> 
    Private Sub Rotate(ByVal axis As Vector3, ByVal angle As Single)
        axis = Vector3.Transform(axis, Matrix.CreateFromQuaternion(Rotation))
        Rotation = Quaternion.Normalize(Quaternion.CreateFromAxisAngle(axis, angle) * Rotation)
    End Sub

    ''' <summary> 
    ''' Translate Object along specific Axis 
    ''' </summary> 
    ''' <param name="distance">Distance to move by</param> 
    Private Sub Translate(ByVal distance As Vector3)
        Position += Vector3.Transform(distance, Matrix.CreateFromQuaternion(Rotation))
    End Sub

#End Region


#Region " Shared Methods "
    ''' <summary> 
    ''' Calculate the rotation to be looking at a point 
    ''' </summary> 
    ''' <param name="position">Origin Vector</param>
    ''' <param name="target">Target Vector</param> 
    ''' <remarks>TODO - clean up this sub!</remarks>
    Public Shared Function DirectionToPoint(ByVal position As Vector3, ByVal target As Vector3) As Quaternion
        Dim tminusp As Vector3 = target - position
        Dim ominusp As Vector3 = Vector3.Forward

        tminusp.Normalize()

        Dim theta As Single = CSng(System.Math.Acos(Vector3.Dot(tminusp, ominusp)))
        Dim cross As Vector3 = Vector3.Cross(ominusp, tminusp)
        Dim x As String = cross.X.ToString()
        Dim y As String = cross.Y.ToString()
        Dim z As String = cross.Z.ToString()

        cross.Normalize()
        If cross.X.ToString().Contains("NaN") Then
            cross.X = 0.0F
        End If
        If cross.Y.ToString().Contains("NaN") Then
            cross.Y = 0.0F
        End If
        If cross.Z.ToString().Contains("NaN") Then
            cross.Z = 0.0F
        End If

        DirectionToPoint = Quaternion.CreateFromAxisAngle(cross, theta)
    End Function
#End Region ' Shared Methods

#Region " Protected Methods "
    Protected Overridable Sub InvalidateMatrices()
        MatricesValid = False
    End Sub
#End Region ' Protected Methods
End Class